/**
* SerialCommand - A Wiring/Arduino library to tokenize and parse commands
* received over a serial port.
*
* Copyright (C) 2012 Stefan Rado
* Copyright (C) 2011 Steven Cogswell <steven.cogswell@gmail.com>
*                    http://husks.wordpress.com
*
* Version 20120522
*
* This library is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this library.  If not, see <http://www.gnu.org/licenses/>.
*/
#include "SerialCommand.h"

/**
* Constructor makes sure some things are set.
*/
SerialCommand::SerialCommand()
	: commandList(NULL),
	commandCount(0),
	defaultHandler(NULL),
	term('\n'),           // default terminator for commands, newline character
	last(NULL)
{
	strcpy(delim, " "); // strtok_r needs a null-terminated string
	clearBuffer();
    precedingBuffer = "";
    anc = 0;
}

/**
* Adds a "command" and a handler function to the list of available commands.
* This is used for matching a found token in the buffer, and gives the pointer
* to the handler function to deal with it.
*/
void SerialCommand::addCommand(const char *command, void(*function)()) {
#ifdef SERIALCOMMAND_DEBUG
	Serial.print("Adding command (");
	Serial.print(commandCount);
	Serial.print("): ");
	Serial.println(command);
#endif

	commandList = (SerialCommandCallback *)realloc(commandList, (commandCount + 1) * sizeof(SerialCommandCallback));
	strncpy(commandList[commandCount].command, command, SERIALCOMMAND_MAXCOMMANDLENGTH);
	commandList[commandCount].function = function;
	commandCount++;
}

/**
* This sets up a handler to be called in the event that the received command string
* isn't in the list of commands.
*/
void SerialCommand::setDefaultHandler(void(*function)(const char *)) {
	defaultHandler = function;
}

/**
* This checks the Serial stream for characters, and assembles them into a buffer.
* When the terminator character (default '\n') is seen, it starts parsing the
* buffer for a prefix command, and calls handlers setup by addCommand() member
*/
void SerialCommand::readSerial() {
    // Reads all received characters.
    int availSize;
    while ((availSize = Serial.available()) > 0) {
        uint8_t u_blk[129] = { 0 };
        Serial.readBytes(u_blk, ((availSize < (int)(sizeof(u_blk) - 1) ? availSize : sizeof(u_blk) - 1)));
        precedingBuffer.concat((const char *)u_blk);
    }

    // Fetch command string terminated by EOC as the term from precedingBuffer
    //  to command buffer.
    unsigned int pc;
    for (pc = 0; pc < precedingBuffer.length(); pc++) {
        char inChar = precedingBuffer.charAt(pc);
#ifdef SERIALCOMMAND_DEBUG
        Serial.print(inChar);   // Echo back to serial stream
#endif
        if (inChar == term) {
            buffer[anc] = '\0';
#ifdef SERIALCOMMAND_DEBUG
            Serial.print("Received: ");
            Serial.println(buffer);
#endif
            // Search for command.
            char *command = strtok_r(buffer, delim, &last);
            if (command != NULL) {
                boolean matched = false;
                for (uint8_t i = 0; i < commandCount; i++) {
#ifdef SERIALCOMMAND_DEBUG
                    Serial.print("Comparing [");
                    Serial.print(command);
                    Serial.print("] to [");
                    Serial.print(commandList[i].command);
                    Serial.println("]");
#endif
                    if (strncmp(command, commandList[i].command, SERIALCOMMAND_MAXCOMMANDLENGTH) == 0) {
#ifdef SERIALCOMMAND_DEBUG
                        Serial.print("Matched Command: ");
                        Serial.println(command);
#endif
                        matched = true;
                        // Execute the stored handler function for the command
                        (*commandList[i].function)();
                        break;
                    }

                }
                if (!matched && (defaultHandler != NULL)) {
                	(*defaultHandler)(command);
                }
            }

            // Dispose current command.
            anc = 0;
            break;
        }
        else if (inChar == '\b') {
            if (anc > 0)
                buffer[--anc] = '\0';
        }
        else if (isprint(inChar)) {
            if (anc < SERIALCOMMAND_BUFFER) {
                buffer[anc++] = inChar;
                buffer[anc] = '\0';
            }
#ifdef SERIALCOMMAND_DEBUG
            else
                Serial.println("Line buffer is full - increase SERIALCOMMAND_BUFFER");
#endif
        }
    }

    // Relocate preceding buffer to the next command string.
    precedingBuffer.remove(0, pc + 1);
    if (precedingBuffer.length() <= 0) {
        precedingBuffer.~String();
        precedingBuffer = "";
    }
}

/*
* Clear the input buffer.
*/
void SerialCommand::clearBuffer() {
	buffer[0] = '\0';
	bufPos = 0;
}

/**
* Retrieve the next token ("word" or "argument") from the command buffer.
* Returns NULL if no more tokens exist.
*/
char *SerialCommand::next() {
	return strtok_r(NULL, delim, &last);
}